#include "BallHeadingOnGoalBehaviour.h"
#include "../../CommonFunctions.h"

/*
	Spravanie brankara v pripade, ze lopta smeruje na branu.
 */
void BallHeadingOnGoalBehaviour::Behave()
{
//	G_TXTN("  LPT_BallheadingOnGoal");
	int degTol=10;
	Point home = GetInterceptBallBehaviour()->GetNearestBallInterceptPos();
	
	if(fabsf(home.GetX()) > 52.45) // it seems that it's impossible to catch it
	{
		// if ball is near, try to catch it anyway
		if((world->me->pos - world->ball->pos).GetLength() < 2.1f)
		{
//			G_TXTN("  Trying to catch it anyway... %.2f", (world->me->pos - world->ball->pos).GetLength());
			GetCatchBehaviour()->Behave();
		}
		else
		{
			// try to catch it at least on goal line :(
			static Point ourGoal;
			ourGoal = world->params->flags[GOAL_L];
			home.SetY(world->ball->pos.GetY() + world->ball->vel.GetY() * (ourGoal.GetX() - world->ball->pos.GetX()) / world->ball->vel.GetX());
			home.SetX(ourGoal.GetX());
		}
	}
	if((home - world->me->pos).GetLength() > 0)
	{
		Ball tmpBall(*world->ball); tmpBall.Step();
		
		Vector meHome = (home - world->me->pos);
		Vector meBall = (tmpBall.pos - world->me->pos);
		float deg; // = fabsf(meHome.GetAngle().GetDeg() - meBall.GetAngle().GetDeg());
		deg = fabsf(meHome.GetAngle().GetDeg() - world->me->GetAbsDir().GetDeg());
		if(deg > 180) deg = 360 - deg;
		if(deg < 110) GetRunToPositionBehaviour()->Behave(home, true, true, 0, degTol);
		else GetRunToPositionBehaviour()->BehaveBackwards(home, true, true, 0, degTol);
/*		G_TXTN("  Running for life from (%.2f, %.2f) to (%.2f, %.2f) p(%.2f, %.2f) v(%.2f, %.2f) %.2f",
			world->me->pos.GetX(), world->me->pos.GetY(),
			home.GetX(), home.GetY(),
			world->ball->pos.GetX(), world->ball->pos.GetY(),
			world->ball->vel.GetX(), world->ball->vel.GetY(),
			(world->me->pos - world->ball->pos).GetLength());

			*/
	}
}
